/**
  ******************************************************************************
  * File Name          : zgt_hand.cpp
  * Description       : zgt_hand program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <zgt_hand/zgt_hand.h>

std::string _port;
serial::Serial ser;

unsigned int step = 0;
unsigned int enables[FIG_DOF] = {0, 0, 0, 0, 0, 0};
unsigned int delays[FIG_DOF] = {0, 0, 0, 0, 0, 0};      // Rate*data ->0.1s*delays[x]
unsigned int fingers[FIG_DOF] = {0, 0, 0 ,0 ,0 ,0};

unsigned char txd[TXD_MAX] = {0x55, 0xaa, 0x04, 0x00, 0x03, 0x37, 0x00, 0x00, 0x00}; 
unsigned char rxd[RXD_MAX];  
                  

/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  
  * @param
  * @retval
  */
  unsigned char seten(unsigned char dt1,unsigned char dt2,unsigned char dt3,unsigned char dt4,unsigned char dt5,unsigned char dt6)
  {
    enables[0] = dt1;
    enables[1] = dt2;
    enables[2] = dt3;
    enables[3] = dt4;
    enables[4] = dt5;
    enables[5] = dt6;
    return 1;
  }

/**
  * @brief  
  * @param
  * @retval
  */
  unsigned char checken(void)
  {
    if(enables[0]==0&&enables[1]==0&&enables[2]==0&&enables[3]==0&&enables[4]==0&&enables[5]==0)
    {
      return 0;
    }
    else
    {
      return 1;
    }
  }

/**
  * @brief  通信数据校验和
  * @param
  * @retval
  */
 unsigned char setpos(unsigned int pos,unsigned char buf[TXD_MAX])
 {
    buf[PTCL_DL] = (unsigned char)pos;
    buf[PTCL_DH] = (unsigned char)(pos>>8);

    return 1;
 }


/**
  * @brief  通信数据校验和
  * @param
  * @retval
  */
  unsigned char checksum(unsigned char buf[TXD_MAX], unsigned char len)
  {
    unsigned int cs=0;
    for(int i = 2; i<len; i++)
    {
      cs+=buf[i];
    }
    buf[PTCL_CS] = (unsigned char)cs;

    return 1;
  }

/**
  * @brief  接收话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void gestureCB(const std_msgs::UInt16::ConstPtr & msg)
{

    if(checken())
    {
      printf("hand busy.\r\n");
    }
    else
    {
      switch(msg->data)
      {
        case 0: //张开
          //手指位置控制
          fingers[0] = MAX_AG;
          fingers[1] = MIN_AG;
          fingers[2] = MAX_AG;
          fingers[3] = MAX_AG;
          fingers[4] = MAX_AG;
          fingers[5] = MAX_AG;

          //手指时序控制
          delays[0] = 0;
          delays[1] = 0;
          delays[2] = 2;
          delays[3] = 2;
          delays[4] = 2;
          delays[5] = 2;

          //手指使能控制
          seten(1,1,1,1,1,1);
          printf("======[0]\r\n");
          break;
        case 1: //抓握(球形 R=0.03m)
          //手指位置控制
          fingers[0] = 800;   //MIN_AG;
          fingers[1] = MAX_AG;
          fingers[2] = 800;   //MIN_AG;
          fingers[3] = 900;  //MIN_AG;
          fingers[4] = 950;   //MIN_AG;
          fingers[5] = 700;   //MIN_AG;

          //手指时序控制
          delays[0] = 3;
          delays[1] = 2;
          delays[2] = 0;
          delays[3] = 0;
          delays[4] = 0;
          delays[5] = 0;

          //手指使能控制
          seten(1,1,1,1,1,1);
          printf("======[1]\r\n");
          break;
      }
    }
}

/**
  * @brief  接收话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void errorCB(const std_msgs::UInt16::ConstPtr & msg)
{
  unsigned char err[]={0x55, 0xAA, 0x03, 0x00, 0x04, 0x00, 0x1E, 0x00};

  if(checken())
  {
    printf("hand busy.\r\n");
  }
  else
  {
    if(msg->data)
    {
      //手指错误清除
      err[PTCL_ID] = 1;
      checksum(txd,(ERR_MAX-1));
      ser.write(err, ERR_MAX);
      usleep(10000);

      err[PTCL_ID] = 2;
      checksum(txd,(ERR_MAX-1));
      ser.write(err, ERR_MAX);
      usleep(10000);

      err[PTCL_ID] = 3;
      checksum(txd,(ERR_MAX-1));
      ser.write(err, ERR_MAX);
      usleep(10000);

      err[PTCL_ID] = 4;
      checksum(txd,(ERR_MAX-1));
      ser.write(err, ERR_MAX);
      usleep(10000);

      err[PTCL_ID] = 5;
      checksum(txd,(ERR_MAX-1));
      ser.write(err, ERR_MAX);

      usleep(10000);
      err[PTCL_ID] = 6;
      checksum(txd,(ERR_MAX-1));
      ser.write(err, ERR_MAX);
      printf("======[clear err]\r\n"); 
    }
  }
}

/**
  * @brief  接收话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void fingerCB(const zgt_msgs::Fingers::ConstPtr & msg)
{
  unsigned int pos = 0;
  if(checken())
  {
    printf("hand busy.\r\n");
  }
  else
  {
    if((msg->id>6)||(msg->id<1))
    {
      printf("finger id error.");
    }
    else
    {
      txd[PTCL_ID] = msg->id;
      pos = msg->position;
      if(pos < MIN_AG)
      {
        pos = MIN_AG;
      }
      if(pos > MAX_AG)
      {
        pos = MAX_AG;
      }
      setpos(pos,txd);
      checksum(txd,(TXD_MAX-1));
      ser.write(txd, TXD_MAX);
    }
  }
}


/**
  * @brief  驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
  ros::init(argc, argv, "zgt_hand");
  ros::NodeHandle nh;
  ros::Rate loop_rate(10);

  nh.getParam("/zgt_hand/port", _port);


  //订阅夹爪话题
  ros::Subscriber sub_gesture = nh.subscribe("zgt_hand/gesture", 100, gestureCB);
  ros::Subscriber sub_error = nh.subscribe("zgt_hand/err", 100, errorCB);
  ros::Subscriber sub_finger = nh.subscribe("zgt_hand/finger", 100, fingerCB);

  try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort(_port.c_str()); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(5); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 

  while (ros::ok())
  {
    if(checken())
    {
      if(enables[step])
      {
        if(delays[step]<=0)
        {
          txd[PTCL_ID] = step+1;
          setpos(fingers[step],txd);
          checksum(txd,(TXD_MAX-1));
          ser.write(txd, TXD_MAX);
          printf("fig(%d):[", txd[PTCL_ID]);
          for(int i=0; i<TXD_MAX; i++)
          {
            printf(" 0x%02x ", txd[i]); 
          }
          printf("]\r\n");
          enables[step] = 0;
          }
        else
        {
          delays[step]-=1;
        }
      }
      step = (step+1)%FIG_DOF;
    }
    ros::spinOnce();
    loop_rate.sleep();
  }

  if(ser.isOpen())
  {
    ser.close();
  }
  
  return 0;
}
